#include "mTreeNode.h"
#include <values.h>
#include <assert.h>
#include <math.h>

void check_ptr_idx_pair(struct store * store, off_t idx, const void * ptr) {
  void * ptr_2 = store_read(store, idx);
  assert(ptr == ptr_2);
  store_release(store, idx);
}

int split(struct mTree * tree, off_t node, struct mTreeNode * node_ptr, off_t object, void * object_ptr);
double get_distance(const struct mTree * tree, const void * query_object_ptr, const off_t tree_object);

double get_distance(const struct mTree * tree, const void * query_object_ptr, const off_t tree_object) {
  const void * tree_object_ptr;
  double dist;

  tree_object_ptr = store_read(tree->oStore, tree_object);
  dist = tree->distance(tree_object_ptr, query_object_ptr);
  store_release(tree->oStore, tree_object);

  return dist;
}



void mTreeNode_query(const struct mTree * tree, const struct mTreeNode* node_ptr,
		     void * queryObject, const double queryDist,
		     void (*processHit)(void *, void *), void * state) {

  double distToNode;
  if(node_ptr->dbObject != ISNULL) {
    distToNode = get_distance(tree, queryObject, node_ptr->dbObject);
  } else {
    assert(node_ptr->parent == ISNULL);
    distToNode = ISNULL;
  }
  if(node_ptr->dbObject == ISNULL ||
     (distToNode - node_ptr->radius) <= queryDist) {
    if(node_ptr->isLeaf) { // node_ptr->children contains object indices.
      int i;
      for(i = 0; i < MTREE_BFACTOR && node_ptr->children[i] != ISNULL; i++) {
	void * child_ptr;
	child_ptr = store_read(tree->oStore, node_ptr->children[i]);

	if(tree->distance(child_ptr, queryObject) <= queryDist) {
	  processHit(child_ptr, state);
	}

	store_release(tree->oStore, node_ptr->children[i]);
      }
    } else {               // node_ptr->children contains node indices
      int i;
      for(i = 0; i < MTREE_BFACTOR && node_ptr->children[i] != ISNULL; i++) {
	struct mTreeNode * child_ptr;
	//	double distanceToChildNode;
	child_ptr = store_read(tree->nStore, node_ptr->children[i]);
	//	distanceToChildNode = get_distance(tree, queryObject, child_ptr->dbObject);
	//	if((distanceToChildNode - child_ptr->radius) <= queryDist) {

	if(node_ptr->dbObject == ISNULL ||
	   (distToNode - (child_ptr->pDistance + child_ptr->radius)) <= queryDist) {
	  mTreeNode_query(tree, child_ptr,
			  queryObject, queryDist,
			  processHit, state);
	}

	store_release(tree->nStore, node_ptr->children[i]);
      }
    }
  }
}

int mTreeNode_size(const struct mTree * tree, const struct mTreeNode * node_ptr) {
  int ret = 0;
  if(node_ptr->isLeaf) {
    while((ret < MTREE_BFACTOR) && (node_ptr->children[ret] != ISNULL)) {
      ret ++;
    }
  } else {
    int i = 0;
    while((i < MTREE_BFACTOR) && (node_ptr->children[i] != ISNULL)) {
      struct mTreeNode * child_ptr = store_read(tree->nStore, node_ptr->children[i]);
      ret += mTreeNode_size(tree, child_ptr);
      store_release(tree->nStore, node_ptr->children[i]);
      i++;
    }
  }

  return ret;
}


/**
   Updates the radius of node.  Calls store_write if necessary.
*/
void mTreeNode_updateRadius(struct mTree * tree, off_t node, struct mTreeNode * node_ptr, double distanceToNode) {
  check_ptr_idx_pair(tree->nStore, node, node_ptr);
  if(distanceToNode > node_ptr->radius) {
    node_ptr->radius = distanceToNode;
    store_write(tree->nStore, node);
  }
}

/**

    This function calculates all pair-wise distances between the
    children of node_ptr and object.  If node_ptr is a leaf, then only
    it's children array will be used.  If it is not a leaf, then it's
    children array, as well as the dbObject field of each of its
    children will be used.  The other fields are ignored.

    @param objects is an output parameter.  It must be MTREE_BFACTOR+1
    elements long.

    @param paird is an output parameter.  It must be a square
    MTREE_BFACTOR+1 by MTREE_BFACTOR+1 element matrix.  If node_ptr is
    a leaf, the elements of paird are the distances between the
    members of objects.  Otherwise, the members are these distances
    plus the corresponding child radius for each distance.
*/
void calculate_paird(off_t objects[MTREE_BFACTOR+1], off_t child[MTREE_BFACTOR+1],double paird[MTREE_BFACTOR+1][MTREE_BFACTOR+1], double radii[MTREE_BFACTOR+1],
		     const struct mTree * tree, const struct mTreeNode * node_ptr,
		     const off_t object, const void * object_ptr) {

  const void * object_ptrs[MTREE_BFACTOR+1];
  //  off_t child[MTREE_BFACTOR+1];
  const struct mTreeNode * child_ptrs[MTREE_BFACTOR+1];
  int i;


  // Initialize the arrays.

  if(node_ptr->isLeaf) {
    check_ptr_idx_pair(tree->oStore, object, object_ptr);

    // Handle the children of node_ptr.  (These are objects in oStore)

    // Since this is a leaf, the child array is the same as the objects array.

    for(i = 0; i < MTREE_BFACTOR; i++) {
      objects[i] = node_ptr->children[i];
      child[i] = objects[i];
      object_ptrs[i] = store_read(tree->oStore, objects[i]);
    }

    // Handle the new object.  (Must be an object)

    objects[MTREE_BFACTOR] = object;
    child[MTREE_BFACTOR] = objects[MTREE_BFACTOR];
    object_ptrs[MTREE_BFACTOR] = object_ptr;
  } else {

    check_ptr_idx_pair(tree->nStore, object, object_ptr);


    // node_ptr's children are nodes.

    for(i = 0; i < MTREE_BFACTOR; i++) {
      child[i] = node_ptr->children[i];
      child_ptrs[i] = store_read(tree->nStore, child[i]);
      objects[i] = child_ptrs[i]->dbObject;
      object_ptrs[i] = store_read(tree->oStore, objects[i]);
    }

    child[MTREE_BFACTOR] = object;
    child_ptrs[MTREE_BFACTOR] = object_ptr;
    objects[MTREE_BFACTOR] = ((struct mTreeNode *)object_ptr)->dbObject;
    object_ptrs[MTREE_BFACTOR] = store_read(tree->oStore, objects[MTREE_BFACTOR]);
  }


  // Calculate the distances.

  for(i = 0; i < MTREE_BFACTOR+1; i++) {
    int j;
    for(j = i; j < MTREE_BFACTOR+1; j++) {
      paird[i][j] = tree->distance(object_ptrs[i], object_ptrs[j]);
      paird[j][i] = paird[i][j];
    }
    //    assert(fabs(paird[i][i]) <  1e-200);
  }

  // Store the radii for later.  (If necessary)
  if(node_ptr->isLeaf) {
    for(i = 0; i < MTREE_BFACTOR+1; i++) {
      radii[i] = 0;
    }
  } else {
    for(i = 0; i < MTREE_BFACTOR+1; i++) {
      radii[i] = child_ptrs[i]->radius;
    }
  }

  // Free any memory that was allocated above.

  if(node_ptr->isLeaf) {
    for(i = 0; i < MTREE_BFACTOR; i++) {  // The last object in objects[] was passed in by the caller--Don't release it.
      store_release(tree->oStore, objects[i]);
    }
  } else {
    for(i = 0; i < MTREE_BFACTOR; i++) {
      store_release(tree->oStore, objects[i]);
      store_release(tree->nStore, child[i]);
    }
    assert(objects[MTREE_BFACTOR] == ((struct mTreeNode*)object_ptr)->dbObject);
    store_release(tree->oStore, ((struct mTreeNode*)object_ptr)->dbObject);
  }
}


/**
   This function implements the "minimum of maximum radius" promotion algorithm, assuming that the generalized hyperplane partition algorithm is in use.
*/
void choose_promotions(int new_parents[2], double paird[MTREE_BFACTOR+1][MTREE_BFACTOR+1], const double radii[MTREE_BFACTOR+1]) {
  double min_radius = MAXDOUBLE;

  int i, j, k;


  for(i = 0; i < MTREE_BFACTOR+1; i++) {
    for(j = i+1; j < MTREE_BFACTOR+1; j++) {
      double max_radius = -1.0;
      for(k = 0; k < MTREE_BFACTOR+1; k++) {
	if(paird[i][k] < paird[j][k]) {
	  if((paird[i][k] + radii[k]) > max_radius) {
	    max_radius = paird[i][k] + radii[k];
	  }
	} else {
	  if(paird[j][k] + radii[k] > max_radius) {
	    max_radius = paird[j][k] + radii[k];
	  }
	}
      }
      if(max_radius < min_radius) {
	min_radius = max_radius;
	new_parents[0] = i;
	new_parents[1] = j;
      }
    }
  }

}
/**
    This function does the following:

    Initialize the following fields of new_node_1 and new_node_2:

    parent, children, dbObject, isLeaf

    The radius fields are initialized to 0, and the elements of children are set to ISNULL. They will be set by the partition function. pDistance will be set by the update_parent function.

    If old_node is the tree root, then it will have the following fields updated:

         children, dbObject, isLeaf

       The following fields of the root pointer never change:

         pDistance = MAXDOUBLE, radius = MAXDOUBLE, parent = ISNULL.

    If old_node is not the tree root, then it is going to be dereferenced soon, and is not updated.

*/
void initialize_promoted_nodes(struct mTree* tree,
			       const off_t old_node, const struct mTreeNode * old_node_ptr,
			       const off_t new[2],   struct mTreeNode * new_ptrs[2],
				 off_t dbObject1, off_t dbObject2) {

  int i;
  struct mTreeNode * tmp;

  check_ptr_idx_pair(tree->nStore, old_node, old_node_ptr);
  check_ptr_idx_pair(tree->nStore, new[0], new_ptrs[0]);
  check_ptr_idx_pair(tree->nStore, new[1], new_ptrs[1]);


  /*  if(old_node_ptr->parent == ISNULL) {
    new_ptrs[0]->parent = old_node;
    new_ptrs[1]->parent = old_node;
  } else {
    new_ptrs[0]->parent = old_node_ptr->parent;
    new_ptrs[1]->parent = old_node_ptr->parent;
    }*/


  for(i = 0; i < MTREE_BFACTOR; i++) {
    new_ptrs[0]->children[i] = ISNULL;
    new_ptrs[1]->children[i] = ISNULL;
  }

  new_ptrs[0]->radius = 0;  // Don't know the real radii yet.  Set to zero for now.
  new_ptrs[1]->radius = 0;

  new_ptrs[0]->dbObject = dbObject1;
  new_ptrs[1]->dbObject = dbObject2;

  new_ptrs[0]->isLeaf = old_node_ptr->isLeaf;
  new_ptrs[1]->isLeaf = old_node_ptr->isLeaf;

  // Update pDistance -- set to MAXDOUBLE for now. update_parent will take care of this.

  new_ptrs[0]->pDistance = MAXDOUBLE;
  new_ptrs[1]->pDistance = MAXDOUBLE;

  tmp = store_read(tree->nStore, new[0]);
  assert(tmp == new_ptrs[0]);
  store_release(tree->nStore, new[0]);

  tmp = store_read(tree->nStore, new[1]);
  assert(tmp == new_ptrs[1]);
  store_release(tree->nStore, new[1]);

  store_write(tree->nStore, new[0]);
  store_write(tree->nStore, new[1]);

}



/**
   This function updates the following fields in old_node (if it is the root node):

   children, isLeaf

   If it is not the root node, then these fields are updated in old_node->parent:

   children, isLeaf,  (radius was already updated by insert)

   The following fields of new_ptr are updated:

   parent, pDistance
*/
void update_parent(struct mTree * tree, off_t old_node, struct mTreeNode * old_node_ptr, off_t new[2], struct mTreeNode* new_ptr[2]) {

  int i;

  check_ptr_idx_pair(tree->nStore, old_node, old_node_ptr);
  check_ptr_idx_pair(tree->nStore, new[0], new_ptr[0]);
  check_ptr_idx_pair(tree->nStore, new[1], new_ptr[1]);

  if(old_node_ptr->parent == ISNULL) {  // We split the root node.

    //// Sanity checks. ------------------------

    // The root node's routing object is null.
    assert(old_node_ptr->dbObject == ISNULL);
    assert(old_node_ptr->pDistance == MAXDOUBLE);
    assert(old_node_ptr->radius == MAXDOUBLE);


    //// Update children nodes. ----------------

    // Set pDistance

    new_ptr[0]->pDistance = 0.0;
    new_ptr[1]->pDistance = 0.0;

    // Set parent  -- Remove -- set once already.

    assert (old_node == 0);

    //    assert(new_ptr[0]->parent == old_node);
    //    assert(new_ptr[1]->parent == old_node);


    //// Update root node. ---------------------

    // The root node is no longer a leaf node. (even if it was before..)
    old_node_ptr->isLeaf = 0;

    // Update children
    old_node_ptr->children[0] = new[0];
    old_node_ptr->children[1] = new[1];

    new_ptr[0]->parent = old_node;
    new_ptr[1]->parent = old_node;


    for(i = 2; i < MTREE_BFACTOR; i++) {
      old_node_ptr->children[i] = ISNULL;
    }

    store_write(tree->nStore, old_node);
    store_write(tree->nStore, new[0]);
    store_write(tree->nStore, new[1]);
  } else {

    off_t parent = old_node_ptr->parent;
    struct mTreeNode * parent_ptr = store_read(tree->nStore, old_node_ptr->parent);
    int i;

    if(parent_ptr->parent == ISNULL) {  // We split a child of the root node.
      new_ptr[0]->pDistance = 0.0;
      new_ptr[1]->pDistance = 0.0;
    } else {
      void * parentObject = store_read(tree->oStore, parent_ptr->dbObject);
      new_ptr[0]->pDistance = get_distance(tree, parentObject, new_ptr[0]->dbObject);
      new_ptr[1]->pDistance = get_distance(tree, parentObject, new_ptr[1]->dbObject);
      store_release(tree->oStore, parent_ptr->dbObject);
    }

    // find old_node and insert new[0] into it's position.

    check_ptr_idx_pair(tree->nStore, parent, parent_ptr);

    for(i = 0; i < MTREE_BFACTOR; i++) {
      if(parent_ptr->children[i] == old_node) {
	check_ptr_idx_pair(tree->nStore, new[0], new_ptr[0]);
	parent_ptr->children[i] = new[0];
	new_ptr[0]->parent = parent;
	//	assert(new_ptr[0]->parent == parent);
	break;
      }
    }
    assert(i < MTREE_BFACTOR);  // old_node isn't a child of it's parent...

    // if there is space in the parent, insert new[1] into it.

    for(; i < MTREE_BFACTOR; i++) {
      if(parent_ptr->children[i] == ISNULL) {
	check_ptr_idx_pair(tree->nStore, new[1], new_ptr[1]);
	parent_ptr->children[i] = new[1];
	new_ptr[1]->parent = parent;
	//	assert(new_ptr[1]->parent == parent);
	break;
      }
    }

    // otherwise, call split.

    store_write(tree->nStore, old_node_ptr->parent);
    store_write(tree->nStore, new[0]);
    store_write(tree->nStore, new[1]);

    assert(new_ptr[0]->parent == parent);

    if(i==MTREE_BFACTOR) { // No room for new[1]
      new_ptr[1]->parent = ISNULL;
      assert(parent_ptr->children[MTREE_BFACTOR-1] != ISNULL);
      check_ptr_idx_pair(tree->nStore, new[1], new_ptr[1]);
      split(tree, old_node_ptr->parent, parent_ptr, new[1], new_ptr[1]);
    }


    store_write(tree->nStore, old_node_ptr->parent);
    store_write(tree->nStore, new[0]);
    store_write(tree->nStore, new[1]);

    store_release(tree->nStore, old_node_ptr->parent);

  }


}


/** Uses generalized hyperplane to partition objects.

    The objects are partitioned according to their proximity to
    promoted_objects.  A new node will always contain its own routing
    object.

    This updates the children and radius fields of the members of new.
    If they are not leaves, it also updates the following fields of
    the children nodes:

    parent, pDistance

    The other fields of the children nodes should not be altered
    during the splitting process.

*/
void partition(struct mTree * tree, off_t new[2], struct mTreeNode * new_ptr[2],
	       int promoted_objects[2], off_t objects[MTREE_BFACTOR+1],
	       double paird[MTREE_BFACTOR+1][MTREE_BFACTOR+1], double radii[MTREE_BFACTOR+1]) {
  int i;
  int idx[] = {0,0};

  check_ptr_idx_pair(tree->nStore, new[0], new_ptr[0]);
  check_ptr_idx_pair(tree->nStore, new[1], new_ptr[1]);



  for(i = 0; i < MTREE_BFACTOR+1; i++) {

    int p = -1;

    if(i == promoted_objects[0]) {
      p = 0;
    } else if(i == promoted_objects[1]) {
      p = 1;
    } else if(paird[i][promoted_objects[0]] < paird[i][promoted_objects[1]]) {
      p = 0;
    } else {
      p = 1;
    }


    // Update the parent's radius, if necessary.

    new_ptr[p]->children[ idx[p] ]=objects[i];
    /*    if(!(new_ptr[p]->isLeaf)) {  // See if block below
      check_ptr_idx_pair(tree->nStore, objects[i], object_ptrs[i]);
      object_ptrs[i]->parent = new[p];
      store_write(tree->nStore, objects[i]);
      }*/

    if(((paird[ i ][ promoted_objects[p] ] + radii[i])) > new_ptr[p]->radius) {
      new_ptr[p]->radius = paird[i][ promoted_objects[p] ] + radii[i];
    }

    // Set childrens' pDistance and parent if applicable.

    if(!(new_ptr[p]->isLeaf)) {
      struct mTreeNode * child = store_read(tree->nStore, new_ptr[p]->children[ idx[p] ]);

      child->parent    = new[p];
      assert(objects[i] == new_ptr[p]->children[ idx[p] ]);

      child->pDistance = paird[i][promoted_objects[p]];

      store_write(tree->nStore,   new_ptr[p]->children[ idx[p] ]);
      store_release(tree->nStore, new_ptr[p]->children[ idx[p] ]);
    }

    idx[p]++;

  }
  store_write(tree->nStore, new[0]);
  store_write(tree->nStore, new[1]);
}

int split(struct mTree * tree,
	  off_t node,   struct mTreeNode * node_ptr,
	  off_t object, void * object_ptr) {

  off_t objects[MTREE_BFACTOR+1];
  off_t children[MTREE_BFACTOR+1];
  double paird[MTREE_BFACTOR+1][MTREE_BFACTOR+1];
  double radii[MTREE_BFACTOR+1];
  off_t promoted[2];
  int   promoted_objects[2];
  struct mTreeNode * promoted_ptrs[2];
  off_t node_ptr_parent = node_ptr->parent;
  int i,j, child_count;


  check_ptr_idx_pair(tree->nStore, node, node_ptr);


  child_count = 0;
  calculate_paird(objects, children, paird, radii, tree, node_ptr, object, object_ptr);

  assert(node_ptr_parent == node_ptr->parent);

  choose_promotions(promoted_objects, paird, radii);

  assert(node_ptr_parent == node_ptr->parent);


  promoted[0] = store_new(tree->nStore);
  promoted[1] = store_new(tree->nStore);

  promoted_ptrs[0] = store_read(tree->nStore, promoted[0]);
  promoted_ptrs[1] = store_read(tree->nStore, promoted[1]);

  initialize_promoted_nodes(tree, node, node_ptr, promoted, promoted_ptrs,
			    objects[promoted_objects[0]], objects[promoted_objects[1]]); // Needs dbObjects for last two parameters.

  assert(node_ptr_parent == node_ptr->parent);



  partition(tree, promoted, promoted_ptrs, promoted_objects, children, paird, radii); // Needs node children for parameter # 5

  assert(node_ptr_parent == node_ptr->parent);


  for(i = 0; i < 2; i++) {
    for(j = 0; j < MTREE_BFACTOR; j++) {
      if(promoted_ptrs[i]->children[j] != ISNULL) {
	child_count ++;
	if(!promoted_ptrs[i]->isLeaf) {
	  struct mTreeNode * n = store_read(tree->nStore, promoted_ptrs[i]->children[j]);
	  assert(n->parent == promoted[i]);
	  store_release(tree->nStore, promoted_ptrs[i]->children[j]);
	}

      }
    }
  }

  assert (child_count == MTREE_BFACTOR+1);

  assert(node_ptr_parent == node_ptr->parent);

  update_parent(tree, node, node_ptr, promoted, promoted_ptrs);

  assert(node_ptr_parent == node_ptr->parent);

  // Shouldn't be necessary, but just in case.
  //  store_write(tree->nStore, promoted[0]);
  //store_write(tree->nStore, promoted[1]);


  store_release(tree->nStore, promoted[0]);
  store_release(tree->nStore, promoted[1]);

  return ISNULL;

}





void mTreeNode_chooseInsertNode(struct mTree * tree, const struct mTreeNode * node_ptr, const void * object_ptr, double distanceToNode,
				int * insertNode, double * insertNodeDistanceToNode) {
  int i;
  /** min_dist is the distance to the closest child that would not require node expansion.  It will be ISNULL if all nodes require expansion. */
  double min_dist = MAXDOUBLE;
  int    min_dist_idx = ISNULL;

  /** min_diff is the distance to the closest child that would require node expansion.  It will be ISNULL if no nodes require expansion. */
  double min_diff     = MAXDOUBLE;
  int    min_diff_idx = ISNULL;

  for(i = 0; i < MTREE_BFACTOR && node_ptr->children[i] != ISNULL; i++) {
    const struct mTreeNode * child_ptr;
    double distanceToChild = -1;
    int is_in_dist = 0;
    child_ptr = store_read(tree->nStore, node_ptr->children[i]);

    if((child_ptr->pDistance - child_ptr->radius) < distanceToNode) {  // If not, then this node can't be a candidate for min_dist

      distanceToChild = get_distance(tree, object_ptr, child_ptr->dbObject);

      if(distanceToChild < child_ptr->radius) {
	is_in_dist = 1;  // Don't consider this for min_diff.

	if(distanceToChild < min_dist) {
	  min_dist = distanceToChild;
	  min_dist_idx = i;
	}
      }
    }

    if((!is_in_dist) &&             // This can't go into min_diff and
       min_dist_idx == ISNULL       // we don't already have something in min_dist
       ) {

      if(distanceToChild < 0) {
	distanceToChild = get_distance(tree, object_ptr, child_ptr->dbObject);
      }

      if(distanceToChild < min_diff) {
	min_diff = distanceToChild;
	min_diff_idx = i;
      }

    }

    store_release(tree->nStore, node_ptr->children[i]);

  }

  assert(min_dist_idx != ISNULL || min_diff_idx != ISNULL);

  if(min_dist_idx != ISNULL) {
    *insertNode =  min_dist_idx;
    *insertNodeDistanceToNode = min_dist;
  } else {
    *insertNode =  min_diff_idx;
    *insertNodeDistanceToNode = min_diff;
  }
  assert(*insertNode != ISNULL && node_ptr->children[*insertNode] != ISNULL);
}


/**
   Inserts an object into the subtree rooted at node.

   This function assume
*/
int mTreeNode_insert(struct mTree * tree,
		     off_t node, struct mTreeNode * node_ptr,
		     off_t object, void * object_ptr,
		     double distanceToNode) {
  check_ptr_idx_pair(tree->nStore, node, node_ptr);

  mTreeNode_updateRadius(tree, node, node_ptr, distanceToNode);
  if(node_ptr->isLeaf) {
    int i = 0;

    // Find first free index in node.
    while((i < MTREE_BFACTOR) && (node_ptr->children[i] != ISNULL)) {
      i++;
    }
    if(i < MTREE_BFACTOR) {                   // Free space in node.

      node_ptr->children[i] = object;
      // node_ptr is a leaf, don't attempt to update children...
      store_write(tree->nStore, node);
      return ISNULL;

    } else {                                  // Node is full.
      return split(tree, node, node_ptr, object, object_ptr);
    }
  } else {

    int insertChild_arrayIndex;
    struct mTreeNode * insertChild_ptr;
    off_t insertChild;
    double insertChildDistanceToNode;

    off_t ret;

    mTreeNode_chooseInsertNode(tree, node_ptr, object_ptr, distanceToNode,
			       &insertChild_arrayIndex, &insertChildDistanceToNode);
    insertChild = node_ptr->children[insertChild_arrayIndex];

    insertChild_ptr = store_read(tree->nStore, insertChild);
    assert(insertChild_ptr->parent == node);
    ret = mTreeNode_insert(tree, insertChild, insertChild_ptr, object, object_ptr, insertChildDistanceToNode);
    //    store_write   (tree->nStore, insertChild);  // Shouldn't be necessary...
    store_release (tree->nStore, insertChild);

    return ret;
  }
}


int mTreeNode_sizeof() {
  return sizeof(struct mTreeNode);
}

void mTreeNode_print(struct mTree * tree, int node, int depth) {
  int i;

  for(i = 0; i < depth; i++) {
    printf("\t");
  }
  if(node == ISNULL) {
    printf("NULL\n");
  } else {
    struct mTreeNode *node_ptr = store_read(tree->nStore, node);
    //    struct xyzi *obj_ptr;
    if(node_ptr->dbObject != ISNULL) {
      //      obj_ptr = store_read(tree->oStore, node_ptr->dbObject);
      printf("#=%d P=%d O=%d R=%f D=%f L=%d\n", node, node_ptr->parent, node_ptr->dbObject, node_ptr->radius, node_ptr->pDistance, node_ptr->isLeaf);
      //      printf("#=%d P=%d O=%d Oi=%d R=%d D=%d L=%d\n", 0, 0, 0, obj_ptr->i, node_ptr->radius, node_ptr->pDistance, node_ptr->isLeaf);
      //      store_release(tree->oStore, node_ptr->dbObject);
    } else {
      printf("#=%d P=%d O=%d Oi=- R=%f D=%f L=%d\n", node, node_ptr->parent, node_ptr->dbObject, node_ptr->radius, node_ptr->pDistance, node_ptr->isLeaf);
    }
    if(node_ptr->isLeaf) {
      for(i =0 ; i <= depth; i++) {
	printf("\t");
      }
      for(i = 0; i < MTREE_BFACTOR; i++) {
	printf("%d ", node_ptr->children[i]);
      }
      printf("\n");
    } else {
      for(i = 0; i < MTREE_BFACTOR; i++) {
	assert(node_ptr->children[i] != 0);
	mTreeNode_print(tree, node_ptr->children[i], depth+1);
      }
    }
    store_release(tree->nStore, node);
  }

}

double mTreeNode_calc_real_radius(struct mTree* tree, const struct mTreeNode * node_ptr, const void * object_ptr) {
  int i = 0;
  double radius = 0.0;
  assert(node_ptr->dbObject != ISNULL);
  if(node_ptr->isLeaf) {
    for(i = 0; i < MTREE_BFACTOR && node_ptr->children[i] != ISNULL; i++) {
      double new_rad = get_distance (tree, object_ptr, node_ptr->children[i]);
      if(new_rad > radius) {
	radius = new_rad;
      }
    }
  } else {
    for(i = 0; i < MTREE_BFACTOR && node_ptr->children[i] != ISNULL; i++) {
      struct mTreeNode * child_ptr = store_read(tree->nStore, node_ptr->children[i]);
      double new_rad = mTreeNode_calc_real_radius(tree, child_ptr, object_ptr);
      store_release(tree->nStore, node_ptr->children[i]);

      if(new_rad > radius) {
	radius = new_rad;
      }
    }
  }
  return radius;
}

void mTreeNode_verify(struct mTree* tree, off_t node, struct mTreeNode * node_ptr) {
  int i;
  int done = 0;
  for(i = 0; i < MTREE_BFACTOR; i++) {
    if(node_ptr->children[i] == ISNULL) {
      done = 1;
    }
    if(done) {
      assert(node_ptr->children[i] == ISNULL);
      }
    assert((node_ptr->children[i] != 0) || node_ptr->isLeaf);
  }


  if(node_ptr->isLeaf) {

    if(node_ptr->dbObject != ISNULL) {
      double max_distance = 0.0;  // If there are no objects, then the radius should be zero, so initialize max_distance to zero.
      void * dbObject_ptr = store_read(tree->oStore, node_ptr->dbObject);

      for(i = 0; i < MTREE_BFACTOR; i++) {
	if(node_ptr->children[i] != ISNULL) {
	  double distance = get_distance(tree, dbObject_ptr, node_ptr->children[i]);
	  assert (distance >= 0);
	  if(distance > max_distance) {
	    max_distance = distance;
	  }
	}
      }
      if(max_distance != node_ptr->radius) {
	double real_radius = mTreeNode_calc_real_radius(tree, node_ptr, dbObject_ptr);
	//	printf("# %d , radius = %f, local radius = %f real radius = %f\n", (int)node, node_ptr->radius, max_distance, real_radius);
	assert(real_radius <=node_ptr->radius*1.0001);
	assert(real_radius <=max_distance*1.0001);
      }

      store_release(tree->oStore, node_ptr->dbObject);
      //      assert(max_distance == node_ptr->radius);
    }
  } else {

    if(node_ptr->dbObject != ISNULL) {
      double max_distance = 0.0;
      void * dbObject_ptr = store_read(tree->oStore, node_ptr->dbObject);
      int isLeaf = ISNULL;
      for(i = 0; i < MTREE_BFACTOR; i++) {
	assert(node_ptr->children[i] != 0);
	if(node_ptr->children[i] != ISNULL) {
	  const struct mTreeNode * child_ptr = store_read(tree->nStore, node_ptr->children[i]);
	  double distance;

	  if(isLeaf == ISNULL) {
	    isLeaf = child_ptr->isLeaf;
	  } else {
	    assert(isLeaf == child_ptr->isLeaf);
	  }

	  assert(child_ptr->dbObject >= 0);

	  distance = get_distance(tree, dbObject_ptr, child_ptr->dbObject);
	  assert(distance >= 0);
	  assert(child_ptr->radius >= 0);
	  if(fabs(distance - child_ptr->pDistance) > distance *.0001) {
	    printf("%f - %f | %x %x - %x %x\n", distance, child_ptr->pDistance, *(unsigned int *)&distance,  *(((unsigned int*)&distance)+1),
		   *(unsigned int*)&child_ptr->pDistance, *(((unsigned int*)&child_ptr->pDistance)+1));
	  }
	  assert(fabs(distance - child_ptr->pDistance) <= distance * .0001);
	  assert(child_ptr->parent == node);
	  if((distance + child_ptr->radius) > max_distance) {
	    max_distance = distance + child_ptr->radius;
	  }
	  store_release(tree->nStore, node_ptr->children[i]);
	}
      }
      if(fabs(max_distance - node_ptr->radius) > max_distance * .0001 ) {
	double real_radius = mTreeNode_calc_real_radius(tree, node_ptr, dbObject_ptr);
	//	printf("# %d , radius = %f, local radius = %f real radius = %f\n", (int)node, node_ptr->radius, max_distance, real_radius);
	assert(real_radius <= (node_ptr->radius * 1.0001));
	assert(real_radius <= (max_distance * 1.0001));
      }

      store_release(tree->oStore, node_ptr->dbObject);
      /*      if(max_distance != node_ptr->radius) {
	printf("# %d , radius = %f, real radius = %f\n", (int)node, node_ptr->radius, max_distance);
	}*/

      //      assert(max_distance == node_ptr->radius);
    } else {
      int isLeaf = ISNULL;
      for(i = 0; i < MTREE_BFACTOR; i++) {
	assert(node_ptr->children[i] != 0);
	if(node_ptr->children[i] != ISNULL) {
	  struct mTreeNode * child_ptr = store_read(tree->nStore, node_ptr->children[i]);

	  if(isLeaf == ISNULL) {
	    isLeaf = child_ptr->isLeaf;
	  } else {
	    assert(isLeaf == child_ptr->isLeaf);
	  }

	  assert(child_ptr->dbObject >= 0);
	  assert(child_ptr->radius >= 0);
	  assert(child_ptr->parent == node);

	  store_release(tree->nStore, node_ptr->children[i]);
	}
      }
    }
    for(i = 0; i < MTREE_BFACTOR; i++) {
      if(node_ptr->children[i] != ISNULL) {
	struct mTreeNode * child_ptr = store_read(tree->nStore, node_ptr->children[i]);
	mTreeNode_verify(tree, node_ptr->children[i], child_ptr);
	store_release(tree->nStore, node_ptr->children[i]);
      }

    }
  }
}
